package roborl.triaLegRobo;

import java.io.File;
import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.io.IOException;
import java.io.ObjectInputStream;
import java.io.ObjectOutputStream;
import java.util.HashMap;
import java.util.Vector;

import net.phys2d.math.Vector2f;
import net.phys2d.raw.Body;
import net.phys2d.raw.FixedMotorJoint;
import net.phys2d.raw.MotorJoint;
import net.phys2d.raw.shapes.Box;
import net.phys2d.raw.shapes.Circle;

import roborl.Action;
import roborl.Decision;
import roborl.DiscreteValueApr;
import roborl.PhysRobot;
import roborl.PhysicsWorld;
import roborl.RobotState;
import roborl.StaticState;
import roborl.ValueApproximate;
import roborl.wheelRobo.SimpleRoboReward;
import roborl.wheelRobo.WheelRobo;

public class ThreeLegMonster extends PhysRobot {
	Body center;
	Body b1, b2, b3;
	// MotorJoint m1, m2, m3;
	FixedMotorJoint m1, m2, m3;

	Vector<Action> allActions;
	{
		allActions = new Vector<Action>();
		for (int i = 0; i < 3; i++) {
			for (int j = 0; j < 3; j++) {
				for (int k = 0; k < 3; k++) {
					allActions.add(new MultiAction(i, j, k));
				}
			}

		}
	}

	static class MultiAction extends Action {
		public int direction[];

		public MultiAction(int... i) {
			super("-" + i[0] + "-" + i[1] + "-" + i[2]);
			direction = i;
		}
	}

	private final double mod(double a, double b) {
		int c = (int) (a / b);
		a -= c * b;
		if (a < 0)
			a += b;
		return a;
	}

	public boolean isIn(double a, double b, double c) {
		double ca = mod(c - a, 2 * Math.PI);
		double ba = mod(b - a, 2 * Math.PI);
		return ca < ba;
	}

	DiscreteValueApr apr;
	float x = 250;
	float y = 300;

	public ThreeLegMonster() {
		super.DECISION_ITERATION = 30;
		super.SILENT_ITERATION = 25;
		apr = new DiscreteValueApr(allActions);
		Decision dec = new Decision(apr, new MonsterReward());
		setDecision(dec);

		float raidus = 20, legLength = 40, legWidth = 5;

		center = new Body(new Circle(raidus), 10);
		center.setPosition(x, y);

		Vector<Object> parts = getParts();
		parts.add(center);
//		float raidus = 20, legLength = 40, legWidth = 20;
//
//		center = new Body(new Circle(raidus), 10);
//		center.setPosition(x, y);
//
//		Vector<Object> parts = getParts();
//		parts.add(center);
//
//		b1 = new Body(new Box(legLength * 1.8f, legWidth), 1);
//		b1.setPosition(x + (float) Math.cos(Math.PI / 2)
//				* (-legLength / 2f + raidus), y
//				+ (float) Math.sin(-Math.PI / 2) * (-legLength / 2f + raidus));
//		b1.setRotation((float) Math.PI / 2f);
		b1 = new Body(new Box(legLength * 1.4f, legWidth), 1);
		b1.setPosition(x + (float) Math.cos(Math.PI / 2)
				* (-legLength / 2f + raidus), y
				+ (float) Math.sin(-Math.PI / 2) * (-legLength / 2f + raidus));
		b1.setRotation((float) Math.PI / 2f);

		b2 = new Body(new Box(legLength, legWidth), 1);
		b2.setPosition(x + (float) Math.cos(Math.PI / 2 + Math.PI * 2 / 3)
				* (legLength / 2f + raidus), y
				- (float) Math.sin(Math.PI / 2 + Math.PI * 2 / 3)
				* (legLength / 2f + raidus));
		b2.setRotation(-(float) (Math.PI / 2 + Math.PI * 2 / 3));

		b3 = new Body(new Box(legLength, legWidth), 1);
		b3.setPosition(x + (float) Math.cos(Math.PI / 2 + Math.PI * 4 / 3)
				* (legLength / 2f + raidus), y
				- (float) Math.sin(Math.PI / 2 + Math.PI * 4 / 3)
				* (legLength / 2f + raidus));
		b3.setRotation(-(float) (Math.PI / 2 + Math.PI * 4 / 3));

		parts.add(b1);
		parts.add(b2);
		parts.add(b3);

		// m1 = new MotorJoint(center, b1, new Vector2f((float) Math
		// .cos(Math.PI / 2)
		// * (raidus), -(float) Math.sin(Math.PI / 2) * (raidus)),
		// new Vector2f(-legLength / 2, 0));
		// parts.add(m1);
		//
		// m2 = new MotorJoint(center, b2, new Vector2f((float) Math.cos(Math.PI
		// / 2 + Math.PI * 2 / 3)
		// * (raidus), -(float) Math.sin(Math.PI / 2 + Math.PI * 2 / 3)
		// * (raidus)), new Vector2f(-legLength / 2, 0));
		// parts.add(m2);
		//
		// m3 = new MotorJoint(center, b3, new Vector2f((float) Math.cos(Math.PI
		// / 2 + Math.PI * 4 / 3)
		// * (raidus), -(float) Math.sin(Math.PI / 2 + Math.PI * 4 / 3)
		// * (raidus)), new Vector2f(-legLength / 2, 0));
		// parts.add(m3);
		Vector<Float> fixSet = new Vector<Float>();
		// for (int i : new int[] { 5, 0, 1 }) {
		// fixSet.add((float) (i / 6.0f * Math.PI * 2));
		// }
		fixSet.add((float) -0.4f);
//		fixSet.add((float) 0f);
		fixSet.add((float) 0.4f);

		m1 = new FixedMotorJoint(center, b1, new Vector2f((float) Math
				.cos(Math.PI / 2)
				* (raidus), -(float) Math.sin(Math.PI / 2) * (raidus)),
				new Vector2f(-legLength / 2, 0));
		m1.setAngleSet(fixSet);
		parts.add(m1);

		m2 = new FixedMotorJoint(center, b2, new Vector2f((float) Math
				.cos(Math.PI / 2 + Math.PI * 2 / 3)
				* (raidus), -(float) Math.sin(Math.PI / 2 + Math.PI * 2 / 3)
				* (raidus)), new Vector2f(-legLength / 2, 0));
		m2.setAngleSet(fixSet);
		parts.add(m2);

		m3 = new FixedMotorJoint(center, b3, new Vector2f((float) Math
				.cos(Math.PI / 2 + Math.PI * 4 / 3)
				* (raidus), -(float) Math.sin(Math.PI / 2 + Math.PI * 4 / 3)
				* (raidus)), new Vector2f(-legLength / 2, 0));
		m3.setAngleSet(fixSet);
		parts.add(m3);
		//		
		m1.setAngularSpeed(0.2f);
		m2.setAngularSpeed(0.2f);
		m3.setAngularSpeed(0.2f);
	}

	@Override
	public Vector<Action> getAvailableActions() {
		// TODO Auto-generated method stub
		Vector<Action> allActions;
		{
			allActions = new Vector<Action>();
			for (int i = 0; i < 3; i++) {
				if (m1.getCurrentAngleSet() == 1 && i == 0)
					continue;
				if (m1.getCurrentAngleSet() == 0 && i == 1)
					continue;

				for (int j = 0; j < 3; j++) {
					if (m2.getCurrentAngleSet() == 1 && j == 0)
						continue;
					if (m2.getCurrentAngleSet() == 0 && j == 1)
						continue;
					for (int k = 0; k < 3; k++) {
						if (m3.getCurrentAngleSet() == 1 && k == 0)
							continue;
						if (m3.getCurrentAngleSet() == 0 && k == 1)
							continue;
						allActions.add(new MultiAction(i, j, k));
					}
				}

			}
		}
		return allActions;
	}

	public void init() {
		float dx, dy;
		dx = x - center.getPosition().getX();
		dy = y - center.getPosition().getY();
		for (Object p : getParts()) {
			if (p instanceof Body) {
				Body b = (Body) p;
				b.adjustPosition(new Vector2f(dx, dy));
			}
		}
	}

	@Override
	public RobotState getState() {
		// TODO Auto-generated method stub
//		int subDivide = 4;
		HashMap<String, Double> s = new HashMap<String, Double>();
		// s.put("m1", (double) (int) (mod(b1.getRotation(), 2 * Math.PI) / 2
		// / Math.PI * subDivide));
		// s.put("m1", (double) (int) (mod(b2.getRotation(), 2 * Math.PI) / 2
		// / Math.PI * subDivide));
		// s.put("m1", (double) (int) (mod(b3.getRotation(), 2 * Math.PI) / 2
		// / Math.PI * subDivide));
		s.put("m1", (double) m1.getCurrentAngleSet());
		s.put("m2", (double) m2.getCurrentAngleSet());
		s.put("m3", (double) m3.getCurrentAngleSet());
		Vector<Double> hs = new Vector<Double>();
		hs.add((double) center.getPosition().getX());
		return new StaticState(s, hs);
	}

	@Override
	protected void simpleUpdate() {
		// TODO Auto-generated method stub
		if (center.getPosition().getX() > 700) {
			lastAction = null;
			for (Object o : parts) {
				if (o instanceof Body) {
					((Body) o).adjustPosition(new Vector2f(-300, 00));
				}

			}
			achieve++;

		} else if (center.getPosition().getX() < 100) {
			lastAction = null;
			for (Object o : parts) {
				if (o instanceof Body) {
					((Body) o).adjustPosition(new Vector2f(300, 00));
				}
			}

		}
	}

	@Override
	public void takeAction(Action action) {
		// TODO Auto-generated method stub
		// System.out.println(action);
		if (action == none) {
			// m1.setAngularSpeed(0);
			// m2.setAngularSpeed(0);
			// m3.setAngularSpeed(0);
			return;
		}
		MultiAction action2 = (MultiAction) action;
		if (action2.direction[0] == 0) {
			int ca = m1.getCurrentAngleSet();
			ca++;
			if (ca > 2)
				ca = 2;
			m1.setCurrentAngleSet(ca);
		} else if(action2.direction[0] == 1){
			int ca = m1.getCurrentAngleSet();
			ca--;
			if (ca < 0)
				ca = 0;
			m1.setCurrentAngleSet(ca);
		}

		if (action2.direction[1] == 0) {
			int ca = m2.getCurrentAngleSet();
			ca++;
			if (ca > 2)
				ca = 2;
			m2.setCurrentAngleSet(ca);
		} else if(action2.direction[1] == 1) {
			int ca = m2.getCurrentAngleSet();
			ca--;
			if (ca < 0)
				ca = 0;
			m2.setCurrentAngleSet(ca);
		}

		if (action2.direction[2] == 0) {
			int ca = m3.getCurrentAngleSet();
			ca++;
			if (ca > 2)
				ca = 2;
			m3.setCurrentAngleSet(ca);
		} else if(action2.direction[2] == 1) {
			int ca = m3.getCurrentAngleSet();
			ca--;
			if (ca < 0)
				ca = 0;
			m3.setCurrentAngleSet(ca);
		}

	}

	public static void main(String s[]) throws InterruptedException {
		final PhysicsWorld physicsWorld = new PhysicsWorld();
		PhysRobot physRobot = new ThreeLegMonster();
		// physRobot.readOnly=true;
		new Thread() {
			public void run() {
				physicsWorld.start();
			}
		}.start();
		physicsWorld.target = 1;
		physRobot.read();
		Thread.sleep(500);
		physicsWorld.addRobot(physRobot);
	}

}
